import cv2
import numpy as np
import serial
import time
ser = serial.Serial("/dev/ttyS5",9600,timeout=1)
cap2 = cv2.VideoCapture(1)
cap2.set(3, 320);cap2.set(4, 240)
def GetColor(lower, upper, lower0, upper0, area, color, hsvimg):
    mask = cv2.inRange(hsvimg, lower, upper)
    mask0 = cv2.inRange(hsvimg, lower0, upper0)
    mask = cv2.add(mask, mask0)
    # 图像学膨胀腐蚀
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.GaussianBlur(mask, (3, 3), 0)
    # 寻找轮廓
    cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    if len(cnts) > 0:
        cnt = max(cnts, key=cv2.contourArea)
        a = cv2.contourArea(cnt)
        if a > area:
            return (color)
        else:
            return ('none')
    else:
        return ('none')

blue_lower = np.array([86, 0, 0]);blue_upper = np.array([132, 255, 255])  # 蓝色范围，最低值;最高值
blue_lower0 = blue_lower;blue_upper0 = blue_upper
red_lower = np.array([0, 100, 0]);red_upper = np.array([20, 255, 255])  # 红色范围，最低值;最高值
red_lower0 = np.array([160, 100, 0]);red_upper0 = np.array([179, 255, 255])
green_lower = np.array([45, 45, 0]);green_upper = np.array([63, 255, 255])  # 绿色范围，最低值;最高值
green_lower0 = green_lower;green_upper0 = green_upper
flag=0
co='n'
while True:
    ret, frame = cap2.read()
    frame = cv2.GaussianBlur(frame, (5, 5), 0)
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    if flag == 0 :
        if GetColor(red_lower, red_upper, red_lower0, red_upper0, 30000, 'r', hsv) == 'r' :
            flag = flag+1
            co='r'
        elif GetColor(blue_lower, blue_upper, blue_lower0, blue_upper0, 30000, 'b', hsv) == 'b' :
            flag = flag+1
            co='b'
        elif GetColor(green_lower, green_upper, green_lower0, green_upper0, 30000, 'g', hsv) == 'g' :
            flag = flag+1
            co='g'
        else :
            co = 'n'
        print (co)
    elif flag < 9 :
        if GetColor(red_lower, red_upper, red_lower0, red_upper0, 30000, 'r', hsv) == co :
            flag = flag+1
            print ('r')
        elif GetColor(blue_lower, blue_upper, blue_lower0, blue_upper0, 30000, 'b', hsv) == co :
            flag = flag+1
            print ('b')
        elif GetColor(green_lower, green_upper, green_lower0, green_upper0, 30000, 'g', hsv) == co :
            flag = flag+1
            print ('g')
    elif flag == 9 :
        ser.write('a'.encode('utf-8'))
        flag = flag+1
    else :
        print('sleep 5s')
        time.sleep(1)
        ret, frame = cap2.read()
        print('sleep 4s')
        time.sleep(1)
        ret, frame = cap2.read()
        print('sleep 3s')
        time.sleep(1)
        ret, frame = cap2.read()
        print('sleep 2s')
        time.sleep(1)
        ret, frame = cap2.read()
        print('sleep 1s')
        time.sleep(1)
        ret, frame = cap2.read()
        flag=0
        co='n'
        pass
